#include <BridgeHelper.h>
#include <iostream>
#include <RTI.hh>
#include <fedtime.hh>
#include <carla/client/TimeoutException.h>
#include <carla/client/Client.h>
#include <carla/client/World.h>
#include <carla/client/BlueprintLibrary.h>
#include <carla/geom/Location.h>
#include <carla/geom/Rotation.h>
#include <carla/geom/Transform.h>
#include <carla/client/ActorList.h>
#include <csignal>
#include "easylogging++.h"
#include "Utils.h"
#include "CarlaSimulation.h"
#include "SumoSimulation.h"
#include "SimulationSynchronization.h"
#include "RtiHelper.h"
#include "Entity.h"



using namespace std;
using namespace carla;

RTI::InteractionClassHandle hSumoClass;

RTI::InteractionClassHandle hSyncClass;

RTIfedTime g_currentTime = 0.0;
bool g_bConstrained = false;
bool g_bRegulation = false;
bool g_granted = false;

SumoSimulation *sumoSimulation;

double carlaWorldTime = 0;

void stopCarlaRtiSignalHandle(int sigintNum);

std::function<void(int)> clearCarlaRti;

void world_ontick_fun(const client::WorldSnapshot &snapshot) {
    carlaWorldTime = snapshot.GetTimestamp().elapsed_seconds;
}

int carla_rti(CarlaArguments carlaArguments) {
    int simulationTime = 3600;

    RtiHelper rtiHelper(carlaArguments);
    rtiHelper.initGlobalFlag(&g_currentTime, &g_bConstrained, &g_bRegulation, &g_granted);

    CarlaSimulation *carlaSimulation = new CarlaSimulation(carlaArguments);
    sumoSimulation = new SumoSimulation();
    SimulationSynchronization *synchronization = new SimulationSynchronization(carlaSimulation, sumoSimulation);


    try {
        RTIambassadorReturn ambassadorReturn = rtiHelper.createFederationExecution();
        if (ambassadorReturn.code != 0)
            return ambassadorReturn.code;
        FederateHandleReturn federateHandleReturn = rtiHelper.joinFederationExecution();
        if (federateHandleReturn.code != 0)
            return federateHandleReturn.code;

        hSumoClass = rtiHelper.getInteractionClassHandle("sumo_car");
        hSyncClass = rtiHelper.getInteractionClassHandle("info_head");

        auto rti = ambassadorReturn.rti;

        RTI::ObjectClassHandle hNetOffsetObject = rti.getObjectClassHandle("net_offset");
        RTI::AttributeHandle hNetOffsetX = rti.getAttributeHandle("x", hNetOffsetObject);
        RTI::AttributeHandle hNetOffsetY = rti.getAttributeHandle("y", hNetOffsetObject);

        RTI::AttributeHandleSet *attributeNetOffset;
        attributeNetOffset = RTI::AttributeHandleSetFactory::create(2);
        attributeNetOffset->add(hNetOffsetX);
        attributeNetOffset->add(hNetOffsetY);

        rti.subscribeObjectClassAttributes(hNetOffsetObject, *attributeNetOffset);
        rtiHelper.subscribeInteractionClass(hSumoClass);
        rtiHelper.subscribeInteractionClass(hSyncClass);

        rtiHelper.enableTimeConstrained();
        rtiHelper.enableTimeRegulation(carlaArguments.stepLength, 0.0001);
        rtiHelper.enableAsynchronousDelivery();

        client::World world = carlaSimulation->getWorld();
        world.OnTick(world_ontick_fun);

        signal(15, stopCarlaRtiSignalHandle);
        clearCarlaRti = [synchronization, carlaSimulation](int signum) {
            delete synchronization;
            delete carlaSimulation;
            delete sumoSimulation;
            LOG(INFO) << "Interrupt signal (" << signum << ") received.";
            exit(signum);
        };

        SharedPtr<client::BlueprintLibrary> blueprint_library = world.GetBlueprintLibrary();

        client::ActorBlueprint vehile_bp = *blueprint_library->Find("vehicle.tesla.model3");
        vehile_bp.SetAttribute("color", "255,255,255");

        SharedPtr<client::Map> map = world.GetMap();
        std::vector<geom::Transform> recommend_points = map->GetRecommendedSpawnPoints();

        client::Vehicle::Control control;
        control.throttle = 0.00;

        world.GetSpectator()->SetTransform(geom::Transform(
            geom::Location(200, 172, 320),
            geom::Rotation(-89, -90, 0)));

        long step = 0;
        long allStep = static_cast<long>(simulationTime / carlaArguments.stepLength);

        rti.requestClassAttributeValueUpdate(hNetOffsetObject, *attributeNetOffset);
        while (step < allStep) {
            LOG(INFO) << "step: " << step <<
                    ", CarlaTime: " << std::fixed << std::setprecision(3) << carlaWorldTime <<
                    ", 仿真时间：" << std::fixed << std::setprecision(3) << g_currentTime;
            LOG(TRACE) << (Utils::Number::lessThan(carlaWorldTime, g_currentTime) ? "需要Carla单独推进" : "不需要Carla单独推进");

            if (Utils::Number::lessThan(carlaWorldTime, g_currentTime)) {
                synchronization->tick();
                step++;
            }
            else if (Utils::Number::moreThan(carlaWorldTime, g_currentTime)) {
                rtiHelper.timeAdvanceRequest();
            }
            else {
                synchronization->tick();
                rtiHelper.timeAdvanceRequest();
            }
        }
        LOG(TRACE) << "退出仿真循环";
        clearCarlaRti(0);
    }
    catch (client::TimeoutException &e) {
        LOG(ERROR) << "连接carla超时: " << e.what() << endl;
        clearCarlaRti(0);
    }
    catch (std::runtime_error &e) {
        LOG(ERROR) << "Error: " << e.what() << endl;
        clearCarlaRti(410);
        return 410;
    }
    catch (RTI::Exception &e) {
        LOG(ERROR) << "FED_HW: ERROR:" << e << endl;
        clearCarlaRti(411);
        return 411;
    }
    return 0;
}

void stopCarlaRtiSignalHandle(int sigintNum) {
    clearCarlaRti(sigintNum);
}

int main(int argc, const char *argv[]) {
    Utils::logConfig(argc, argv);
    const CarlaArguments carlaArguments = Utils::parameterAnalysis(argc, argv);
    carla_rti(carlaArguments);
    return 0;
}
